{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib notebook  \n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "from quadrotor2d.quadrotor2d import Quadrotor2DVisualizer\n",
    "\n",
    "viz = Quadrotor2DVisualizer()\n",
    "context = viz.CreateDefaultContext()\n",
    "x = np.array([0, 0, .5, 0, 0, 0])\n",
    "context.FixInputPort(0, x)\n",
    "viz.draw(context)\n",
    "\n",
    "R = np.array([[ np.cos(x[2]), -np.sin(x[2])], [np.sin(x[2]), np.cos(x[2])]])\n",
    "\n",
    "plt.rcParams.update({'font.size': 24})\n",
    "viz.ax.plot([0,1.5*viz.length],[0,0],'k', linewidth=1)\n",
    "viz.ax.plot([0,1.5*viz.length*np.cos(x[2])], [0, 1.5*viz.length*np.sin(x[2])],'k', linewidth=1)\n",
    "viz.ax.annotate('($x$,$y$)', xy=(0, 0), xytext=(0.05,-.15), arrowprops={\"arrowstyle\": \"->\"})\n",
    "t = np.linspace(0,x[2],25)\n",
    "viz.ax.plot(.75*viz.length*np.cos(t), 0.75*viz.length*np.sin(t),'k', linewidth=1)\n",
    "viz.ax.annotate(r'$\\theta$', xy=(.8*viz.length*np.cos(x[2]/2), 0.8*viz.length*np.sin(x[2]/2)-.03))\n",
    "pts = np.dot(R,np.array([[viz.length, viz.length],[0, 0.8*viz.length]]))\n",
    "viz.ax.arrow(pts[0,0],pts[1,0],pts[0,1]-pts[0,0],pts[1,1]-pts[1,0], color=[.6,.2,.2], width=0.01)\n",
    "viz.ax.annotate('$u_1$', xy=pts[:,1]+.02)\n",
    "pts = np.dot(R,np.array([[-viz.length, -viz.length],[0, 0.8*viz.length]]))\n",
    "viz.ax.arrow(pts[0,0],pts[1,0],pts[0,1]-pts[0,0],pts[1,1]-pts[1,0], color=[.6,.2,.2], width=0.01)\n",
    "viz.ax.annotate('$u_2$', xy=pts[:,1]+.02)\n",
    "viz.ax.arrow(-2*viz.length,.3,0,-.3, color='k', width=0.005)\n",
    "viz.ax.annotate('$g$', xy=(-1.9*viz.length, .2))\n",
    "viz.ax.set_title('')\n",
    "viz.ax.set_xlim(-2.1*viz.length, 1.5*viz.length)\n",
    "viz.ax.set_ylim(-.05, 0.2)\n",
    "\n",
    "plt.axis('off')\n",
    "plt.gca().set_position([0, 0, 1, 1])\n",
    "plt.show()\n",
    "\n",
    "plt.savefig(\"quadrotor2d.svg\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
